package drone.iqgc.example ;

import java.util.concurrent.CountDownLatch ;
import java.util.concurrent.TimeUnit ;

import org.slf4j.Logger ;
import org.slf4j.LoggerFactory ;

import io.mavsdk.System ;
import io.mavsdk.action.Action ;
import io.mavsdk.telemetry.Telemetry ;
import io.mavsdk.telemetry.Telemetry.Health ;
import io.mavsdk.telemetry.Telemetry.Position ;
import io.reactivex.Completable ;

public class TakeoffToHigh
{

	private static final Logger logger = LoggerFactory.getLogger ( TakeoffToHigh.class ) ;

	public static void main (
			String [ ] args
	)
	{
		logger.debug ( "Starting example: takeoff and land..." ) ;

		CountDownLatch latch = new CountDownLatch ( 1 ) ;

		System drone = new System ( "60.205.58.145" , 50042 ) ;
		
		drone.getTelemetry().getFlightMode() .subscribe(flightMode -> logger.info("Flight mode: " + flightMode));
		
//		Position position=drone.getTelemetry ( ).getPosition ( ).blockingFirst ( );
//		  Float absHigh=position.getAbsoluteAltitudeM ( );
//		  Float relHigh=position.getRelativeAltitudeM ( );
//		  logger.info ( absHigh.toString ( ) ) ;
//		  logger.info ( relHigh.toString ( ) ) ;
		  
		 
		Health health=		drone.getTelemetry ( ).getHealth ( ).blockingFirst ( );
		
		Boolean s1=health.getIsAccelerometerCalibrationOk ( );
		Boolean s2=health.getIsGlobalPositionOk ( );
		Boolean s3=health.getIsGyrometerCalibrationOk ( );
		Boolean s4=health.getIsHomePositionOk ( );
		Boolean s5=health.getIsLocalPositionOk ( );
		Boolean s6=health.getIsMagnetometerCalibrationOk ( );
		
	 
		logger.info ("getIsAccelerometerCalibrationOk : "+ s1.toString ( ) ) ;
		logger.info ("getIsGyrometerCalibrationOk : "+ s3.toString ( ) ) ;
		logger.info ("getIsMagnetometerCalibrationOk : "+ s6.toString ( ) ) ;
		logger.info ("getIsGlobalPositionOk : "+ s2.toString ( ) ) ;
		logger.info ("getIsHomePositionOk : "+ s4.toString ( ) ) ;
		logger.info ("getIsLocalPositionOk : "+ s5.toString ( ) ) ;
		
		logger.info ( drone.getTelemetry ( ).getBattery ( ).blockingFirst ( ).getRemainingPercent ( ).toString ( ) );
		
		
		//logger.info ( drone.getTelemetry ( ).getHealth ( ).blockingFirst ( ).getIsGlobalPositionOk ( ).toString ( ) );
		
		java.lang.System.out.println ("getHealthAllOk : " + drone.getTelemetry ( ).getHealthAllOk ( ).blockingFirst ( ) ) ;
		
		drone.getTelemetry().getFlightMode() .subscribe(flightMode -> logger.info("Flight mode: " + flightMode));
		
//		Position position=drone.getTelemetry ( ).getPosition ( ).blockingFirst ( );
//		  Float absHigh=position.getAbsoluteAltitudeM ( );
//		  Float relHigh=position.getRelativeAltitudeM ( );
//		  logger.info ( absHigh.toString ( ) ) ;
//		  logger.info ( relHigh.toString ( ) ) ;
		  
	
		logger.info ("getIsAccelerometerCalibrationOk : "+ s1.toString ( ) ) ;
		logger.info ("getIsGlobalPositionOk : "+ s2.toString ( ) ) ;
		logger.info ("getIsGyrometerCalibrationOk : "+ s3.toString ( ) ) ;
		logger.info ("getIsHomePositionOk : "+ s4.toString ( ) ) ;
		logger.info ("getIsLocalPositionOk : "+ s5.toString ( ) ) ;
		logger.info ("getIsMagnetometerCalibrationOk : "+ s6.toString ( ) ) ;
		
		logger.info ( drone.getTelemetry ( ).getBattery ( ).blockingFirst ( ).getRemainingPercent ( ).toString ( ) );
		
		
		//logger.info ( drone.getTelemetry ( ).getHealth ( ).blockingFirst ( ).getIsGlobalPositionOk ( ).toString ( ) );
		
		java.lang.System.out.println ( drone.getTelemetry ( ).getHealthAllOk ( ) ) ;
		
		  drone.getTelemetry().getFlightMode()
          .subscribe(flightMode -> logger.info("Flight mode: " + flightMode));
		
		  
		  drone.getTelemetry ( ).getPosition ( ).throttleFirst(3, TimeUnit.SECONDS).subscribe (abc ->{
			logger.info ( abc.getAbsoluteAltitudeM ( )+"---" +abc.getRelativeAltitudeM ( ) ) ;
		} , throwable -> {} );
		  
		  Position position=drone.getTelemetry ( ).getPosition ( ).blockingFirst ( );
		  Float absHigh=position.getAbsoluteAltitudeM ( );
		  Float relHigh=position.getRelativeAltitudeM ( );
		  logger.info ( absHigh+"绝对高度" +relHigh+"相对高度"  ) ;
		  float targetAltitude = 400.0f;
		  drone.getAction ( ).arm ( )
			.doOnComplete ( ( ) -> logger.debug ( "Arming..." ) )
			.doOnError ( throwable -> logger.error ( "Failed to arm: " + ( ( Action.ActionException ) throwable ).getCode ( ) ) )
			.andThen ( 
					drone.getAction ( ).setTakeoffAltitude ( 1f )
					.doOnComplete ( ( ) -> logger.debug ( "set Taking off 1f ..." ) )
					.doOnError ( throwable -> logger.error ( " set take off Failed to take off: " + ( ( Action.ActionException ) throwable ).getCode ( ) ) ) 
				)
			.andThen ( 
					drone.getAction ( ).takeoff ( )
					.doOnComplete ( ( ) -> logger.debug ( "Taking off..." ) )
					.doOnError ( throwable -> logger.error ( "Failed to take off: " + ( ( Action.ActionException ) throwable ).getCode ( ) ) ) 
				)
			.delay ( 30 , TimeUnit.SECONDS )
			.andThen ( 
					drone.getAction ( ).land ( )
					.doOnComplete ( ( ) -> logger.debug ( "Landing..." ) )
					.doOnError ( throwable -> logger.error ( "Failed to land: " + ( ( Action.ActionException ) throwable ).getCode ( ) ) ) 
				)
			.subscribe ( latch::countDown , throwable -> latch.countDown ( ) ) ;

			try
			{
				latch.await ( ) ;
			} catch ( InterruptedException ignored )
			{
				// This is expected
			}
		  
//		  Completable takeoff = drone.getAction().takeoff();
//
//		  takeoff.andThen(drone.getTelemetry().getLandedState()
//		            .filter(state -> {
//		            		logger.info ( state +"" );
//		            	return true;
//		            	
//		            })
//		            .firstOrError())
//		            .flatMapCompletable(landedState -> {
//		            	logger.info("Takeoff completed, climbing to " + targetAltitude + " meters");
//		                // 指定无人机上升到目标高度
//		                return drone.getAction().gotoLocation(
//		                    drone.getTelemetry().getPosition().blockingFirst().getLatitudeDeg(),
//		                    drone.getTelemetry().getPosition().blockingFirst().getLongitudeDeg(),
//		                    targetAltitude,
//		                    drone.getTelemetry().getAttitudeEuler ( ).blockingFirst ( ).getYawDeg ( )
//		                );
//		            })
//		            .subscribe(
//		                () -> logger.info("Climbed to target altitude: " + targetAltitude + " meters"),
//		                throwable -> logger.info("Failed to reach target altitude: " + throwable)
//		            );
//
//		try
//		{
//			latch.await ( ) ;
//		} catch ( InterruptedException ignored )
//		{
//			// This is expected
//		}
		//
	}
}
